#include "BoWModel.h"

using namespace std;

BoWModel::BoWModel(RGBDCamera ** cameras,int nr_cameras,string codebookFileName,int kmeans_iter,int nr_clusters){
	printf("BoWModel\n");
	float ** all_surfs = new float*[10000000];
	int *    all_lap   = new int[10000000];
	int nr_surfs = 0;

    cvNamedWindow("img_col", CV_WINDOW_AUTOSIZE);
	CvSURFParams params = cvSURFParams(1000, 1);
	for(int i = 0; i < nr_cameras; i++)
	{
		printf("datapath:%s\nnr frames: %i\n",(cameras[i]->data_path).c_str(),cameras[i]->nr_frames);
		for(int j = 0; j < cameras[i]->nr_frames; j+=10)
		{
			
			printf("%s\n",(cameras[i]->rgb_list[j])->c_str());
			IplImage* img = cvLoadImage( (cameras[i]->rgb_list[j])->c_str(), CV_LOAD_IMAGE_GRAYSCALE);
			CvMemStorage* storage = cvCreateMemStorage(0);
			CvSeq *imageKeypoints = 0;
			CvSeq *imageDescriptors = 0;
			cvExtractSURF( img, 0, &imageKeypoints, &imageDescriptors, storage, params );
			CvSeqReader reader, kreader;
			cvStartReadSeq( imageKeypoints, &kreader );
			cvStartReadSeq( imageDescriptors, &reader );
			vector<CvPoint2D32f> pt1, pt2;
			for(int l = 0; l < imageKeypoints->total; l++ )
			{
				const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
				const float* descriptor = (const float*)reader.ptr;
				CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
				CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
				int descriptor_length 				= (int)(reader.seq->elem_size/sizeof(float));
				float * desc					= new float[descriptor_length];
				for(int k = 0; k < descriptor_length; k++){desc[k] = descriptor[k];}
				all_surfs[nr_surfs] = desc;
				all_lap[nr_surfs] = kp->laplacian;
				nr_surfs++;
			}
			cvReleaseMemStorage(&storage);
			cvRelease((void **)&imageKeypoints);
			cvReleaseImage( &img);
		}
	}
	printf("nr_surfs so far: %i\n",nr_surfs);
	
	//Clustering time...
	
	printf("Clustering %d samples each with %d dimensions \n", nr_surfs, 128);
	vector<Descriptor> descriptors_lap_pos;
	vector<Descriptor> descriptors_lap_neg;
	printf("Descriptors setup\n");
	float pos = 0;
	float neg = 0;
	unsigned int dimensions = 128;
	for (unsigned int i = 0; i < nr_surfs; i++)
	{
		printf("iter:%i\n",i);
		// Add a descriptor to the list
		if(all_lap[i] == 1)
		{
			descriptors_lap_pos.push_back(Descriptor());
			Descriptor &curDescr = descriptors_lap_pos.back();
			curDescr.resize(dimensions);
			curDescr.setZero();
			for (unsigned int j = 0; j < dimensions; j++)
			{
				curDescr.setValue(j,all_surfs[i][j]);
			}
			pos++;
		}
		else
		{
			descriptors_lap_neg.push_back(Descriptor());
			Descriptor &curDescr = descriptors_lap_neg.back();
			curDescr.resize(dimensions);
			curDescr.setZero();
			for (unsigned int j = 0; j < dimensions; j++)
			{
				curDescr.setValue(j,all_surfs[i][j]);
			}
			neg++;
		}
	}
	printf("pos: %f , neg: %f\n",pos,neg);
	int pos_nr_clusters = ((float)nr_clusters) * pos/(pos+neg);
	int neg_nr_clusters = nr_clusters - pos_nr_clusters;
	printf("pos_nr_clusters: %i , neg_nr_clusters: %i\n",pos_nr_clusters,neg_nr_clusters);
	// Generate the codebook
	cout << "Generating codebook pos: " << endl;
	Codebook codebook_pos(pos_nr_clusters, 128);
	codebook_pos.generate(&descriptors_lap_pos, kmeans_iter);
	cout << "Done!" << endl << endl;

	// Write codebook to file
	cout << "Writing codebook to file... ";
	string codebookFileName_pos = codebookFileName+"_pos";
	CodebookFile codebookFile_pos(codebookFileName_pos);
	if (!codebookFile_pos.openForWriting())
	{
		cerr<<"Error: Cannot open the codebook file \""<<codebookFileName_pos<<"\"."<<endl;
		//return 1;
	}
	if (!codebookFile_pos.writeCodebook(&codebook_pos))
	{
		cerr<<"Error: Error while writing the codebook file \""<<codebookFileName_pos<<"\"."<<endl;
		//return 1;
	}
	codebookFile_pos.close();
	cout << "Done!" << endl;
	// Finish
	cout<<endl<<"Finished pos!"<<endl;
	
	Codebook codebook_neg(neg_nr_clusters, 128);
	codebook_neg.generate(&descriptors_lap_neg, kmeans_iter);
	cout << "Done!" << endl << endl;

	// Write codebook to file
	cout << "Writing codebook to file... ";
	string codebookFileName_neg = codebookFileName+"_neg";
	CodebookFile codebookFile_neg(codebookFileName_neg);
	if (!codebookFile_neg.openForWriting())
	{
		cerr<<"Error: Cannot open the codebook file \""<<codebookFileName_neg<<"\"."<<endl;
		//return 1;
	}
	if (!codebookFile_neg.writeCodebook(&codebook_neg))
	{
		cerr<<"Error: Error while writing the codebook file \""<<codebookFileName_neg<<"\"."<<endl;
		//return 1;
	}
	codebookFile_neg.close();
	cout << "Done!" << endl;
	// Finish
	cout<<endl<<"Finished!"<<endl;
	exit(0);
}
BoWModel::~BoWModel(){}
